/*
 * Copyright (c) 2020 - ~, HIT_HERO Team
 *
 * STEERING ENGINE MODULE SOUCE FILE
 * Used in RT-Thread Operate System
 *
 * Change Logs:
 * Date           Author       Notes            	Mail
 * 2020-12-18     WangXi       first version    	WangXi_chn@foxmail.com
 */
 
#include "Module_Steering.h"
 
/* Static Method */
static void Module_SteeringInit(MODULE_STEERING *module);
static void Module_SteeringEnable(MODULE_STEERING *module);
static void Module_SteeringDisable(MODULE_STEERING *module);
static void Module_SteeringSetPulse(MODULE_STEERING *module,rt_uint32_t pwm_pulse_ns);
static void Module_SteeringSetAngle(MODULE_STEERING *module,rt_int16_t angle);

 /* Global Method */
rt_err_t Module_Steering_Config(MODULE_STEERING *Dev_Steering){
    if(	Dev_Steering->Method_Init		==	NULL &&
		Dev_Steering->Method_SetPulse	==	NULL &&
		Dev_Steering->Method_Enable		== 	NULL &&
		Dev_Steering->Method_Disable	==	NULL &&
		Dev_Steering->Method_SetAngle	== 	NULL 

    ){  
        /* Link the Method */
        Dev_Steering->Method_Init 		= Module_SteeringInit;
		Dev_Steering->Method_Enable		= Module_SteeringEnable;
		Dev_Steering->Method_Disable	= Module_SteeringDisable;
		Dev_Steering->Method_SetPulse	= Module_SteeringSetPulse;
		Dev_Steering->Method_SetAngle	= Module_SteeringSetAngle;

    }
    else{
        rt_kprintf("Warning: Module Steering is Configed twice\n");
        return RT_ERROR;
    }

    /* Device Init */
    Dev_Steering->Method_Init(Dev_Steering);
    return RT_EOK;
}

/* Static Method */
static void Module_SteeringInit(MODULE_STEERING *module)
{   
    module->pwm_dev = (struct rt_device_pwm *)rt_device_find(module->Property_PwmDevName);
    
    if (module->pwm_dev == RT_NULL)
        rt_kprintf("pwm init failed! can't find pwm device!\n");
    
	switch(module->Property_SteerType){
		case SG90:
			module->Value_PWM_PERIOD_NS = 20000000;//20ms = 20000000 ns
			break;
		case TYPE_STEERING_END:
			break;
		default:
			break;
	}
	
	module->Method_SetAngle(module,module->Property_InitAngle);
	
}

static void Module_SteeringSetPulse(MODULE_STEERING *module,rt_uint32_t pwm_pulse_ns){   
    
	module->Value_PWM_PULSE_NS = pwm_pulse_ns;
    rt_pwm_set(	module->pwm_dev, module->Property_PWM_CHANNEL, 
				module->Value_PWM_PERIOD_NS, module->Value_PWM_PULSE_NS);
  
}
 
static void Module_SteeringEnable(MODULE_STEERING *module){
	
	rt_pwm_enable(module->pwm_dev, module->Property_PWM_CHANNEL);

}

static void Module_SteeringDisable(MODULE_STEERING *module){
	
	rt_pwm_disable(module->pwm_dev, module->Property_PWM_CHANNEL);
	
}
 
static void Module_SteeringSetAngle(MODULE_STEERING *module,rt_int16_t angle){
	
	angle = angle >= module->Property_AngleMAX ? module->Property_AngleMAX:angle;
	angle = angle <= 0 ? 0:angle;
	
	module ->Value_Angle = angle;
	double k = (module->Property_PulseMAXus - module->Property_PulseMINus)
				/module->Property_AngleMAX;
	
	module->Value_PWM_PULSE_NS = 
				(k * module ->Value_Angle + module->Property_PulseMINus)*1000;
	
    rt_pwm_set(	module->pwm_dev, module->Property_PWM_CHANNEL, 
				module->Value_PWM_PERIOD_NS, module->Value_PWM_PULSE_NS);

}
 
 /************************ (C) COPYRIGHT 2020 WANGXI **************END OF FILE****/
